#include "boat_datadef.h"
#include <math.h>
#include <string.h>

void Dot2Line(float dot[2], float line[2][2], float tardot[2], float unit[2])
{
	float lineerr[2] = {0}, dott[2]={0}, errdot1[2] = {0}, errdot2[2] = {0};
	float a, b, x2, y2, predata, predata1 = 0;
	float doterr1[2] = {0}, doterr2[2] = {0};
	lineerr[0] = line[1][0] - line[0][0];
	lineerr[1] = line[1][1] - line[0][1];
	predata = sqrtf(lineerr[0]*lineerr[0]+lineerr[1]*lineerr[1]);
	unit[0] = lineerr[0] / predata;
	unit[1] = lineerr[1] / predata;
	a = unit[0];
	b = unit[1];
	
	x2 = a*a*dot[0]+b*b*line[0][0]+a*b*(dot[1]-line[0][1]);
	y2 = b*b*dot[1]+a*a*line[0][1]+a*b*(dot[0]-line[0][0]);
	dott[0] = x2;
	dott[1] = y2;
	
	errdot1[0] = line[0][0] - dott[0];
	errdot1[1] = line[0][1] - dott[1];
	errdot2[0] = line[1][0] - dott[0];
	errdot2[1] = line[1][1] - dott[1];
	predata = sqrtf(errdot1[0]*errdot1[0]+errdot1[1]*errdot1[1]);
	errdot1[0] = errdot1[0]/predata;
	errdot1[1] = errdot1[1]/predata;
	predata = sqrtf(errdot2[0]*errdot2[0]+errdot2[1]*errdot2[1]);
	errdot2[0] = errdot2[0]/predata;
	errdot2[1] = errdot2[1]/predata;
	
	errdot1[0] -= errdot2[0];
	errdot1[1] -= errdot2[1];
	predata = sqrtf(errdot1[0]*errdot1[0]+errdot1[1]*errdot1[1]);
	if(predata < 0.001)
	{
		doterr1[0] = line[0][0] - dot[0];
		doterr1[1] = line[0][1] - dot[1];
		doterr2[0] = line[1][0] - dot[0];
		doterr2[1] = line[1][1] - dot[1];
		predata = sqrtf(doterr1[0]*doterr1[0]+doterr1[1]*doterr1[1]);
		predata1 = sqrtf(doterr2[0]*doterr2[0]+doterr2[1]*doterr2[1]);
		if(predata < predata1)
		{
			tardot[0] = line[0][0];
			tardot[1] = line[0][1];
		}
		else
		{
			tardot[0] = line[1][0];
			tardot[1] = line[1][1];
		}
	}
	else
	{
		tardot[0] = dott[0];
		tardot[1] = dott[1];
	}
}


